/*
 * FreenectAdaptee.cpp
 *
 */

#include "../../include/kinect/FreenectAdaptee.h"
#include "../../include/Config.h"

namespace libfreenect {

/*
 * Implementazione dell'inner-class FreenectMutex.
 */

FreenectAdaptee::FreenectMutex::FreenectMutex() {
	_m_mutex = new pthread_mutex_t;
	pthread_mutex_init(_m_mutex, NULL);
}

FreenectAdaptee::FreenectMutex::~FreenectMutex() {
	delete _m_mutex;

	// Prevenzione del dangling reference.
	_m_mutex = NULL;
}

void FreenectAdaptee::FreenectMutex::lock() {
	pthread_mutex_lock(_m_mutex);
}

void FreenectAdaptee::FreenectMutex::unlock() {
	pthread_mutex_unlock(_m_mutex);
}

/*
 * Implementazione della classe FreenectAdaptee.
 */

FreenectAdaptee::FreenectAdaptee(freenect_context *_ctx, int _index)
	: Freenect::FreenectDevice(_ctx, _index),
		_m_buffer_rgb(FREENECT_VIDEO_RGB),
		_m_buffer_depth(FREENECT_DEPTH_11BIT),
		_m_gamma(2048),
		_rgbMat(cv::Size(FRAME_WIDTH, FRAME_HEIGHT), CV_8UC3),
		_depthMat(cv::Size(FRAME_WIDTH, FRAME_HEIGHT), CV_16UC1),
		_ownMat(cv::Size(FRAME_WIDTH, FRAME_HEIGHT), CV_8UC3),
		_m_rgb_mutex(new FreenectMutex()),
		_m_depth_mutex(new FreenectMutex()),
		_m_new_rgb_frame(false),
		_m_new_depth_frame(false)
{
	for (unsigned int i = 0; i < 2048; i++) {
		float v = i / 2048.0;
		v = std::pow(v, 3) * 6;
		_m_gamma[i] = v * 6 * 256;
	}
}

FreenectAdaptee::~FreenectAdaptee() {
	_m_buffer_depth.clear();
	_m_buffer_rgb.clear();
	_m_gamma.clear();
	_rgbMat.release();
	_depthMat.release();
	_ownMat.release();
	delete _m_rgb_mutex;
	delete _m_depth_mutex;

	// Prevenzione del dangling reference.
	_m_rgb_mutex = NULL;
	_m_depth_mutex = NULL;
}

void FreenectAdaptee::VideoCallback(void* _rgb, uint32_t timestam) {
	assert(_m_rgb_mutex != NULL);

	_m_rgb_mutex->lock();
	uint8_t* rgb = static_cast<uint8_t*> (_rgb);
	_rgbMat.data = rgb;
	_m_new_rgb_frame = true;
	_m_rgb_mutex->unlock();
}

void FreenectAdaptee::DepthCallback(void* _depth, uint32_t timestamp) {
	assert(_m_depth_mutex != NULL);

	_m_depth_mutex->lock();
	uint16_t* depth = static_cast<uint16_t*> (_depth);
	_depthMat.data = (uchar*) depth;
	_m_new_depth_frame = true;
	_m_depth_mutex->unlock();
}

bool FreenectAdaptee::getVideo(cv::Mat& dest) {
	assert(_m_rgb_mutex != NULL);

	_m_rgb_mutex->lock();
	bool flag = false;

	if (_m_new_rgb_frame) {
		cv::cvtColor(_rgbMat, dest, CV_RGB2BGR);
		_m_new_rgb_frame = false;
		_m_rgb_mutex->unlock();
		flag = true;
	} else
		_m_rgb_mutex->unlock();

	return flag;
}

bool FreenectAdaptee::getDepth(cv::Mat& dest) {
	assert(_m_depth_mutex != NULL);

	_m_depth_mutex->lock();
	bool flag = false;

	if (_m_new_depth_frame) {
		_depthMat.copyTo(dest);
		_m_new_depth_frame = false;
		_m_depth_mutex->unlock();
		flag = true;
	} else
		_m_depth_mutex->unlock();

	return flag;
}



} /* namespace libfreenect */
